#include "ubt_com/ubt_com_util.h"
#include "ubt_state/ubt_state_task_charge.h"
#include "ubt_state/ubt_state_api.h"
#include "std_msgs/msg/u_int32.hpp"
#include "ubt_state/node.h"
#include "ubt_com/ubt_charge_api.h"

namespace ubt_state
{
struct ChargeClsData
{
    rclcpp::Publisher<ubt_interface::msg::ChargeMsgV1>::SharedPtr _pub_charge_cmd;
    rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr _pub_charge_conn;
    
    rclcpp::Subscription<ubt_interface::msg::ChargeMsgV1>::SharedPtr _sub_charge_result;
    CTaskCharge *_pCurTask;
};
static ChargeClsData g_clsData;

void Charge_Result_CallbackWrap(const ubt_interface::msg::ChargeMsgV1::SharedPtr msg){
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Charge_Result_Callback(msg);
    }
}

int CTaskCharge::ClassInit()
{
    ChargeClsData *pClsData = &g_clsData;
    pClsData->_pub_charge_conn = node->create_publisher<std_msgs::msg::UInt32>("/charging_connect", 1);
    pClsData->_pub_charge_cmd = node->create_publisher<ubt_interface::msg::ChargeMsgV1>("/ubt_charge_node/msg_v1_in", 1);
    pClsData->_sub_charge_result = node->create_subscription<ubt_interface::msg::ChargeMsgV1>("/ubt_charge_node/msg_v1_out", 10, std::bind(&Charge_Result_CallbackWrap, std::placeholders::_1));
    pClsData->_pCurTask = NULL;
    return 0;
}

void CTaskCharge::ClassCleanup()
{
    ChargeClsData *pClsData = &g_clsData;
    // pClsData->_pub_charge_cmd->Shutdown();
    // pClsData->_pub_charge_conn->Shutdown();
    // pClsData->_sub_charge_result->Shutdown();
    pClsData->_pCurTask = NULL;
}

ITask *CTaskCharge::CreateTask(ITaskCb *pTaskCb, std::string taskId, geometry_msgs::msg::Pose2D poseDst, double dis, int index, int reportIndex, std::string chargeType, int chargeTime)
{
    CTaskCharge *pTask = new CTaskCharge();
    if (!pTask->Init(pTaskCb, taskId, poseDst, dis, index, reportIndex, chargeType, chargeTime))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskCharge::~CTaskCharge()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    g_clsData._pCurTask = NULL;
}


bool CTaskCharge::Init(ITaskCb *pTaskCb, std::string taskId, geometry_msgs::msg::Pose2D poseDst, double dis, int index, int reportIndex, std::string chargeType, int chargeTime)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s poseDst=(%lf,%lf,%lf) dis=%lf chargeTime=%d",
             taskId.c_str(), poseDst.x, poseDst.y, poseDst.theta, dis, chargeTime);
    _pTaskCb = pTaskCb;
    _taskId = taskId;
    _index = index;
    _reportIndex = reportIndex;
    _poseDst = poseDst;
    _dis = dis;
    _state = ubt_task_state_idle;
    _msgId = 1;
    _chargePhase = ubt_task_charge_phase_idle;
    _phaseTime = 0;
    _avoidHwStatus = 0;
    _pauseState = 0;
    _stopReason = 0;
    _chargeTime = chargeTime;
    _chargeType = chargeType;
    g_clsData._pCurTask = this;
    return true;
}

bool CTaskCharge::Start()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state != ubt_task_state_idle)
    {
        return false;
    }
    _state = ubt_task_state_running;
    _phaseTime = ubt_com::GetNowInMsecs();
    return true;
}

void CTaskCharge::Pause()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_running)
    {
        _state = ubt_task_state_pause;
        _CheckPause(2);
    }
}

void CTaskCharge::Continue()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_pause)
    {
        _state = ubt_task_state_running;
        _CheckPause(3);
    }
}

void CTaskCharge::Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"_CTaskCharge::Stop state=%d, chargePhase=%d, reason=%d", _state, _chargePhase, reason);
    if (_state >= ubt_task_state_stopping)
    {
        return;
    }
    _stopReason = reason;
    if (_chargePhase == ubt_task_charge_phase_idle)
    {
        _state = ubt_task_state_closed;
        _pTaskCb->NotifyTaskStateAndStage(ubt_task_state_closed, GetStage(), reason, "");
        return;
    }
    _state = ubt_task_state_stopping;
    ubt_interface::msg::ChargeMsgV1 chargeMsg;
    if (reason == ubt_task_result_usr_cancel)
    {
        chargeMsg.msg_type = ubt_msg_type_req_cancelcharge;
    }
    else
    {
        chargeMsg.msg_type = ubt_msg_type_req_stopcharge;
    }
    _chargePhase = ubt_task_charge_phase_idle;
    SendChargeMsg(chargeMsg);
}

void CTaskCharge::OnTimer()
{
    long now;
    if (_state == ubt_task_state_running)
    {
        now = ubt_com::GetNowInMsecs();
        if ((_chargePhase == ubt_task_charge_phase_idle) && ((now - _phaseTime) >= 3000))
        {
            char buf[256];
            StateStatus *pStatus = _pTaskCb->GetStateStatus();
            _avoidHwStatus = pStatus->avoidHwStatus;
            _CheckPause(0);
            _chargePhase = ubt_task_charge_phase_enter;
            _phaseTime = now;
            ubt_interface::msg::ChargeMsgV1 chargeMsg;
            chargeMsg.msg_type = ubt_msg_type_req_startcharge; //开始充电
            int flag = 0;
            geometry_msgs::msg::Pose2D poseOff = ubt_com::getL2BPose(pStatus->curPose, _poseDst);
            RCLCPP_INFO(node->get_logger(),"taskId=%s curPose=(%lf,%lf,%lf) poseDst=(%lf,%lf,%lf) poseOff=(%lf,%lf,%lf) _origDis=%lf _avoidHwStatus=%d",
                     _taskId.c_str(), pStatus->curPose.x, pStatus->curPose.y, pStatus->curPose.theta,
                     _poseDst.x, _poseDst.y, _poseDst.theta,
                     poseOff.x, poseOff.y, poseOff.theta, _dis, _avoidHwStatus);
            if (_pauseState == 1)
            {
                flag |= ubt_charge_flag_pause;
            }
            sprintf(buf, "dis=%lf&flag=%d", _dis + poseOff.x, flag);
            chargeMsg.ext_info = std::string(buf);
            SendChargeMsg(chargeMsg);
        }
    }
}

void CTaskCharge::OnMsg(int msgType, void *pvMsg)
{
    do
    {
        if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
        {
            break;
        }
        if (msgType == ubt_msg_type_avoid_status)
        {
            StateStatus *pStatus = _pTaskCb->GetStateStatus();
            _avoidHwStatus = pStatus->avoidHwStatus;
            _CheckPause(1);
            break;
        }
    }
    while (false);
}

ubt_task_type CTaskCharge::GetType()
{
    return ubt_task_type_charge;
}

ubt_task_state CTaskCharge::GetState()
{
    ubt_task_state state = _state;
    if ((state == ubt_task_state_running) ||
            (state == ubt_task_state_stopping))
    {
        if ((_avoidHwStatus == 1) && (_chargePhase == ubt_task_charge_phase_leave))
        {
            state = ubt_task_state_obstacle;
        }
    }
    if ((state == ubt_task_state_obstacle) || (state == ubt_task_state_pause))
    {
        if (_chargePhase == ubt_task_charge_phase_charge)
        {
            state = ubt_task_state_running;
        }
    }
    return state;
}

ubt_task_stage CTaskCharge::GetStage()
{
    if (_chargePhase == ubt_task_charge_phase_enter)
    {
        return ubt_task_stage_recharge2;
    }
    else if (_chargePhase == ubt_task_charge_phase_charge)
    {
        return ubt_task_stage_charge;
    }
    else if (_chargePhase == ubt_task_charge_phase_idle)
    {
        return ubt_task_stage_run;
    }
    else
    {
        return ubt_task_stage_recharge;
    }
}

std::string CTaskCharge::GetActionType()
{
    return "charge";
}

void CTaskCharge::GetIndex(int& index, int& reportIndex)
{
    index = _index;
    reportIndex = _reportIndex;
}

void CTaskCharge::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    stopReason = _stopReason;
    stopReasonDesc = _stopReasonDesc;
}

void CTaskCharge::GetLogs(std::vector<TaskLogItem>& logs)
{

}

void CTaskCharge::Charge_Result_Callback(const ubt_interface::msg::ChargeMsgV1::SharedPtr msg)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"CTaskCharge::Charge_Result_Callback state: " << _state << " msg: " << msg);
    if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
    {
        return;
    }
    if (_chargePhase == ubt_task_charge_phase_idle)
    {
        return;
    }
    if (msg->msg_type == ubt_msg_type_nty_chargestatus)//充电状态切换
    {
        if (msg->value1 == ubt_charge_status_charging)
        {
            if ((_chargePhase == ubt_task_charge_phase_enter) || (_chargeTime != -1))
            {
                _chargePhase = ubt_task_charge_phase_charge;
                _pTaskCb->NotifyTaskStateAndStage(ubt_task_state_running, GetStage(), 0, "");
            }
        }
        else if (msg->value1 == ubt_charge_status_leaving && _chargeType != "uslam") 
        {
            _chargePhase = ubt_task_charge_phase_leave;
            _CheckPause(4);
        }
        else if (msg->value1 == ubt_charge_status_leaving && _chargeType == "uslam")
        {
            _chargePhase = ubt_task_charge_phase_leave;
        }
    }
    if (msg->msg_type == ubt_msg_type_rsp_charge_rsp) //充电结束
    {
        _chargePhase = ubt_task_charge_phase_idle;
        _state = ubt_task_state_closed;
        int closeReason = _stopReason;
	    if(msg->value1 == ubt_charge_result_failbtystatus)//失败就结束任务
        {
            /*_state = ubt_task_state_closed;
                _pTaskCb->NotifyTaskStateAndStage(ubt_task_state_closed, GetStage(),ubt_task_result_charge_abort, "");
            ubt_charge_pkg::chargeMsgV1 chargeMsg;
            chargeMsg.msgType = ubt_msg_type_req_stopcharge;
                SendChargeMsg(chargeMsg);
            */
            Stop(ubt_task_result_charge_abort);
            std_msgs::msg::UInt32 charge_msg;
            charge_msg.data = 0;
            g_clsData._pub_charge_conn->publish(charge_msg);
            
            return;
        }
        if (_stopReason == 0)  // ubt_task_result_ok = 0
        {
            if (msg->value1 != ubt_charge_result_ok)
            {
                closeReason = ubt_task_result_charge_abort;
            }
        }
        _pTaskCb->NotifyTaskStateAndStage(_state, GetStage(), closeReason, "");
    }
}

void  CTaskCharge::SendChargeMsg(ubt_interface::msg::ChargeMsgV1& chargeMsg)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"CTaskCharge::SendChargeMsg state: " << _state << " msg: " << chargeMsg.msg_id );
    chargeMsg.msg_id = _msgId;
    _msgId++;
    chargeMsg.task_id = _taskId;
    chargeMsg.charge_time = _chargeTime;
    chargeMsg.charge_type = _chargeType;
    g_clsData._pub_charge_cmd->publish(chargeMsg);
}

void CTaskCharge::_CheckPause(int event)
{
    int oldPauseState = _pauseState;
    _pauseState = 0;
    if (((_avoidHwStatus == 1) && (_chargePhase == ubt_task_charge_phase_leave)) ||
            (_state == ubt_task_state_pause))
    {
        _pauseState = 1;
    }
    if (_chargePhase == ubt_task_charge_phase_idle)
    {
        return;
    }
    if (oldPauseState != _pauseState)
    {
        ubt_interface::msg::ChargeMsgV1 chargeMsg;
        if (_pauseState != 0)
        {
            chargeMsg.msg_type = ubt_msg_type_req_pause;
        }
        else
        {
            chargeMsg.msg_type = ubt_msg_type_req_continue;
        }
        RCLCPP_INFO(node->get_logger(),"CTaskCharge::_CheckPause _pauseState=%d event=%d oldPauseState=%d", _pauseState, event, oldPauseState);
        SendChargeMsg(chargeMsg);
        _pTaskCb->NotifyTaskStateAndStage(GetState(), GetStage(), 0, "");
    }
}



}



